﻿#pragma once

#include <memory>

#include "CoordinateSys.h"
#include "CartesianCRD.hpp"
#include "GeodeticCRD.hpp"

namespace ns_Position
{
    class TopoCentricCRD : public CoordinateSys
    {
    public:
        using PositionType = double;

    public:
        TopoCentricCRD(PositionType northing, PositionType easting, PositionType upping, const CartesianCRD &central)
            : _northing(northing),
              _easting(easting),
              _upping(upping),
              _central(central)
        {
        }

        TopoCentricCRD(const CartesianCRD &central)
            : _central(central)
        {
        }

        bool ToCartesianCRD(CartesianCRD *out) const override
        {
            // TODO
            return false;
        }

        bool FromCartesianCRD(const CartesianCRD &ref) override
        {

            CartesianCRD::PositonType x = ref.GetX();
            CartesianCRD::PositonType y = ref.GetY();
            CartesianCRD::PositonType z = ref.GetZ();

            CartesianCRD::PositonType _centralX = _central.GetX();
            CartesianCRD::PositonType _centralY = _central.GetY();
            CartesianCRD::PositonType _centralZ = _central.GetZ();

            double deltaX = x - _centralX;
            double deltaY = y - _centralY;
            double deltaZ = z - _centralZ;

            std::unique_ptr<GeodeticCRD> GeoCRD(new GeodeticCRD());
            GeoCRD->FromCartesianCRD(CartesianCRD(_centralX, _centralY, _centralZ));

            GeodeticCRD::PositionType B = GeoCRD->GetB() * Mathf::Pi / 180;
            GeodeticCRD::PositionType L = GeoCRD->GetL() * Mathf::Pi / 180;
            GeodeticCRD::PositionType H = GeoCRD->GetH();   

            Matrix matrix1 = Matrix::MatrixContainerType(
                {{-Mathf::Sin(B) * Mathf::Cos(L), -Mathf::Sin(B) * Mathf::Sin(L), Mathf::Cos(B)},
                 {-Mathf::Sin(L), Mathf::Cos(L), 0},
                 {Mathf::Cos(B) * Mathf::Cos(L), Mathf::Cos(B) * Mathf::Sin(L), Mathf::Sin(B)}});
            Matrix matrix2 = Matrix::MatrixContainerType(
                {{deltaX},
                 {deltaY},
                 {deltaZ}});

            Matrix ans = Matrixf::MulMatrix(matrix1, matrix2);

            _northing = ans[0][0];
            _easting = ans[1][0];
            _upping = ans[2][0];

            return true;
        }

        // 获取北向坐标
        PositionType GetNorthing() const
        {
            return _northing;
        }

        // 获取东向坐标
        PositionType GetEasting() const
        {
            return _easting;
        }

        // 获取高度
        PositionType GetUpping() const
        {
            return _upping;
        }

        CartesianCRD GetCentral() const
        {
            return _central;
        }

    private:
        PositionType _northing; // 北向坐标
        PositionType _easting;  // 东向坐标
        PositionType _upping;   // 高度

        CartesianCRD _central; // 站心坐标
    };
}